#include "BspMotors.h"
#include "timer.h"
#include "can_moto_protocol.h"

#define BSP_M_CS(X)      X ? (GPIOA->BSRR = GPIO_Pin_4) : (GPIOA->BRR = GPIO_Pin_4)
#define BSP_M_NRST(X)    X ? (GPIOB->BSRR = GPIO_Pin_0) : (GPIOB->BRR = GPIO_Pin_0)
#define BSP_M_NFREEZE(X) X ? (GPIOB->BSRR = GPIO_Pin_1) : (GPIOB->BRR = GPIO_Pin_1)
#define BSP_M_DEN(X)     X ? (GPIOB->BSRR = GPIO_Pin_11) : (GPIOB->BRR = GPIO_Pin_11)
#define CSDELAY_COUNT   10
static void M_NFREEZE(uint8_t reset) /*motor halt low active*/
{
	if(reset)
		BSP_M_NFREEZE(0);
	else
		BSP_M_NFREEZE(1);
}
TypeDefTMC4361_RegStruct TMC4361_MRegStruct={
	.bsp_write_reg = M_HardWareSendDat,
	.bsp_read_reg = M_HardWareReadDat,
	.PROTO_V = 805500,/* 转速*895 这个值大概900转*/
	.bsp_nfreeze = M_NFREEZE,
	.speed_home = 10,/*10是原本复位速度 其他数值按比例算*/
};

/*********************************************************************************************
  * @brief  BspMotorsCsDelay
  * @param  uint32_t t 延时参数
  * @retval None
	* @IO None
  ********************************************************************************************/
static void BspMotorsCsDelay(uint32_t t)
{
	while(t--)
	{
		__nop();
	}
}

void Delayus(uint32_t t)
{while(t--);}

void Delayms(uint32_t t)
{
	while(t--)
	{
		Delayus(200000);
	}
}

/******************************************************************************
* @brief  BspMotors_IO_Conifg
* @param  None
* @retval None
* @IO PA4---------------M_CS  
* @IO PB0---------------M_NRST
* @IO PB1---------------M_NFREEZE
* @IO PB2---------------M_INTR
* @IO PB10--------------M_TARGET
* @IO PB11--------------M_DEN
*******************************************************************************/
static void BspMotors_IO_Conifg(void)
{
	GPIO_InitTypeDef GPIOX;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);
	
	GPIOX.GPIO_Pin = GPIO_Pin_4;
	GPIOX.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIOX.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIOX);
	
	GPIOX.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_11;
	GPIO_Init(GPIOB,&GPIOX);
	
	GPIOX.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_10;
	GPIOX.GPIO_Mode = GPIO_Mode_IPU;
	GPIO_Init(GPIOB,&GPIOX);
	
	GPIOX.GPIO_Pin = GPIO_Pin_8;
	GPIOX.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_Init(GPIOA,&GPIOX);
	RCC_MCOConfig(RCC_MCO_HSE);
	BSP_M_NFREEZE(1);
	BSP_M_DEN(0);
	Delayms(50);  
//	vTaskDelay(50);
	BSP_M_NRST(0);
	Delayms(50);
//	vTaskDelay(50);
	BSP_M_NRST(1);
//	vTaskDelay(50); 
	Delayms(1);
}

/*********************************************************************************************
  * @brief  M_SPI_Config 驱动器基本配置
  * @param  None
  * @retval None
	* @IO None
  ********************************************************************************************/
void M_SPI_Config(void)
{
	BspSPI1_Config();
	BspMotors_IO_Conifg();

}

/*********************************************************************************************
  * @brief  TMC_CHECK_WRITE 发送数据到驱动器
  * @param  uint8_t RegAddr  寄存器地址
  * @param  uint32_t Data 需要发送的数据
  * @retval uint8_t 驱动状态
	* @IO None
  ********************************************************************************************/
uint32_t M_HardWareSendDat(uint8_t RegAddr,uint32_t Data)
{
	__disable_irq();
	uint8_t Status[5];
	BSP_M_CS(0);
	BspMotorsCsDelay(CSDELAY_COUNT);
	RegAddr |= 0X80;
	Status[0] = Bsp_SPI1_SendByte(RegAddr);
	Status[4] = Bsp_SPI1_SendByte((Data >> 24) & 0X000000FF);
	Status[3] = Bsp_SPI1_SendByte((Data >> 16) & 0X000000FF);
	Status[2] = Bsp_SPI1_SendByte((Data >> 8) & 0X000000FF);
	Status[1] = Bsp_SPI1_SendByte(Data & 0X000000FF);
	BSP_M_CS(1);
	BspMotorsCsDelay(CSDELAY_COUNT);
	__enable_irq();
	return *(uint32_t*)&Status[1];
}

/*********************************************************************************************
  * @brief  M_HardWareReadDat  读取驱动器数据
  * @param  uint8_t RegAddr 寄存器地址
  * @param  uint32_t *Data 读取的数据
  * @retval uint8_t 驱动状态
  * @IO None
  ********************************************************************************************/
uint8_t M_HardWareReadDat(uint8_t RegAddr,uint32_t *Data)
{
	__disable_irq();
	uint8_t Status = 0;
	BSP_M_CS(0);
	BspMotorsCsDelay(CSDELAY_COUNT);
	Bsp_SPI1_SendByte(RegAddr);
	Bsp_SPI1_SendByte(0);
	Bsp_SPI1_SendByte(0);
	Bsp_SPI1_SendByte(0);
	Bsp_SPI1_SendByte(0);
	BSP_M_CS(1);
	BspMotorsCsDelay(CSDELAY_COUNT);
	
	BSP_M_CS(0);
	BspMotorsCsDelay(CSDELAY_COUNT);
	Status = Bsp_SPI1_SendByte(RegAddr);
	*Data = Bsp_SPI1_SendByte(0);
	(*Data) <<= 8;
	*Data |= Bsp_SPI1_SendByte(0);
	(*Data) <<= 8;
	*Data |= Bsp_SPI1_SendByte(0);
	(*Data) <<= 8;
	*Data |= Bsp_SPI1_SendByte(0);
	BSP_M_CS(1);
	BspMotorsCsDelay(CSDELAY_COUNT);
	__enable_irq();
	return Status;
}

/*********************************************************************************************
  * @brief  BspMotors_SetEn  驱动器使能
  * @param  None
  * @retval None
	* @IO None
  ********************************************************************************************/
void BspMotors_SetEn(void)
{
//	BSP_M_DEN(0);
}

/*********************************************************************************************
  * @brief  BspMotors_ResetEn  驱动器失能
  * @param  None
  * @retval None
	* @IO None
  ********************************************************************************************/

static uint32_t last_write = 0;
void TMC_CHECK_WRITE(uint32_t reg,uint32_t value)	
{
	uint32_t read_out = M_HardWareSendDat(reg,value);
	if(last_write != read_out)
	{
		M_STATUS_SET(&TMC4361_MRegStruct.PROTO_STATUS,M_STATUS_SPI_ERR);
	}
	last_write = value;
}
void BspMotors_ResetEn(void)
{
//	BSP_M_DEN(1);
}
static void bsp_2160_cfg(void)
{
	M_SPI_Config();
	
	M_HardWareSendDat(0x4F,0x52535400); //reset
	Delayms(10);
	//vTaskDelay(100);
	//general setup:
	//----------------------
	TMC_CHECK_WRITE(0X03,0X540022);      // Inputfilter: START und Encoderpins   输入滤波器配置
	TMC_CHECK_WRITE(0X04,0X8440000d);   //Setup of SPI Output Timing  SPI输出配置寄存器
	TMC_CHECK_WRITE(0X00,0x6026);       // direct-a, direct-bow  通用配置寄存器 这个寄存器偶尔会异常变成0 导致位置运动加速度特别小
	TMC_CHECK_WRITE(0X10,0X60004);       // step length   步进方向输出配置寄存器
	//configuration of TMC2130:   2130 配置寄存器
//	uint32_t cnt = 100;
//	while(cnt--){
//		TMC_CHECK_WRITE(0X01,0);
//	}
	//----------------------
	
	TMC_CHECK_WRITE(0X6D,0X81);      // GCONF
	TMC_CHECK_WRITE(0X6C,0X07);       // diag1=index, pushpull, direct_mode = on --> SPI mode
	TMC_CHECK_WRITE(0X6D,0X80);      // GCONF
	TMC_CHECK_WRITE(0X6C,0X12200);       // diag1=index, pushpull, direct_mode = on --> SPI mode
	TMC_CHECK_WRITE(0X6D,0X90);      // IHOLD_IRUN
	TMC_CHECK_WRITE(0X6C,0X00070603);        // ihold=0, iholddelay=7, irun=2
	TMC_CHECK_WRITE(0X6D,0X91);     // TPOWERDOWN
	TMC_CHECK_WRITE(0X6C,0Xa);           // TZEROWAIT=10
	TMC_CHECK_WRITE(0X6D,0X94);      // TCOOLTHRS
	TMC_CHECK_WRITE(0X6C,0Xc8);          // =200
	TMC_CHECK_WRITE(0X6D,0X95);      // THIGH
	TMC_CHECK_WRITE(0X6C,0X32);          // =50
	TMC_CHECK_WRITE(0X6D,0Xee);      // DCCTRL
	TMC_CHECK_WRITE(0X6C,0X70025);       // DCCTRLSG=5, DC_TIME=37
	TMC_CHECK_WRITE(0X6D,0Xf0);      // PWMCONF
	TMC_CHECK_WRITE(0X6C,0X404ff);       // 
	TMC_CHECK_WRITE(0X6D,0Xec);      // CHOPCONF 电机锁紧
	TMC_CHECK_WRITE(0X6C,0X10223);       //reset default 0x10410150
	
	TMC_CHECK_WRITE(0X6D,0Xed);      // COOLCONF (off)
	TMC_CHECK_WRITE(0X6C,0X100a220);     //
	TMC_CHECK_WRITE(0X6D,0X8B);      // GLOBAL_SCALER (off)
	TMC_CHECK_WRITE(0X6C,0X0);   //          global scaler = 0
}
static void bsp_4361_cfg(void)
{
	//清零速度位置目标位置
	TMC_CHECK_WRITE(0X24,0X0);  // v=0      VMAX  
	TMC_CHECK_WRITE(0X21,0X0);  // xact=0   XACTUAL
	TMC_CHECK_WRITE(0X37,0X0);  // xtarg=0  XTARGET
	TMC_CHECK_WRITE(0X50,0);    // enc_pos

#if MOTOR_OPENLOOP
#else
	//cl basic parameter  闭环基础参数配置
	//----------------------
	TMC_CHECK_WRITE(0X54,4000);        // encoder resolution = 40k/rev ADAPT THIS PARAMETER to SETUP   编码器分辨率配置寄存器
	TMC_CHECK_WRITE(0X1C,0Xff00ff);      // CL_BETA=CL_GAMMA=255  闭环调节
	TMC_CHECK_WRITE(0X5C,0X10000);      // CL_DELTA_P = 1.0    PID_D

	//cl calibration:  闭环校准
	//----------------------
	TMC_CHECK_WRITE(0X20,0X6);       // ramp mode: hold+pos mode     运行模式
	TMC_CHECK_WRITE(0X24,0X100000);      // slow vel cl calibration: vmax
	
	TMC_CHECK_WRITE(0X07,0X400000);      // regulation_modus: turn on cl  编码器信号配置寄存器
//	TMC_CHECK_WRITE(0X37,0X80);      // move to FS position      目标位置寄存器
	TMC_CHECK_WRITE(0X07,0X1400000);     // cl_calibration_en: turn on cl calibration    编码器信号配置寄存器
	Delayms(20);
	//vTaskDelay(200);
	TMC_CHECK_WRITE(0X07,0X400000);      // cl_calibration_en: turn off cl calibration   编码器配置寄存器
	TMC_CHECK_WRITE(0X24,0X0);       // v=0    VMAX
#endif

	//S-shaped Ramps setup:  S 加减速参数配置
	//----------------------
	TMC_CHECK_WRITE(0X20,0X6);       // ramp mode: hold+pos mode     运行模式
	TMC_CHECK_WRITE(0X24,0X05350000);       // vmax=400kpps    VMAX
	TMC_CHECK_WRITE(0X20,0X06);       // s-ramp + pos mode     运行模式

	//ramp generator    S 曲线斜坡发生器参数
	TMC_CHECK_WRITE(0X28,30000);     // amax
	TMC_CHECK_WRITE(0X29,30000);     // dmax 
	TMC_CHECK_WRITE(0X2D,0x300);     // bow1
	TMC_CHECK_WRITE(0X2E,0X300);     // bow2
	TMC_CHECK_WRITE(0X2F,0X300);     // bow3
	TMC_CHECK_WRITE(0X30,0x300);     // bow4

#if MOTOR_OPENLOOP
#else
	//cl operation setup:     闭环设置
	//----------------------
	// back-emf Compensation   反电动势补偿设置
	TMC_CHECK_WRITE(0X60,250000);       // emf_vmin=
	TMC_CHECK_WRITE(0X61,450000);       // emf_vadd= -> emf_vmax= 
	TMC_CHECK_WRITE(0X62,0Xffffff);      // emc_vel0_timer (enc_vel_zero, clock cycles)   编码器延迟时间
	TMC_CHECK_WRITE(0X63,0X2000864);     // enc vel filter settings (v_enc_mean)   编码器速度配置寄存器
									
	// position Deviation
	TMC_CHECK_WRITE(0X5F,5);      // cl_tolerance=20     PID 参数寄存器
	TMC_CHECK_WRITE(0X5C,0X10000);       // cl_p=1.0     PID_D
									
	// catch up velocity   追赶速度参数设置
	TMC_CHECK_WRITE(0X5A,200);      // cl_vlimit_p=200     PID_P
	TMC_CHECK_WRITE(0X5B,50);      // cl_vlimit_i=50       PID_I
	TMC_CHECK_WRITE(0X5D,1000);     // cl_vlimit_diclip=1000   PID_D
	TMC_CHECK_WRITE(0X5E,200000);       // cl_vlimit=100kpps         PID限幅寄存器

	// closed loop scaling   闭环电流调节参数设置
	TMC_CHECK_WRITE(0X06,0x32ff64);      // cl scaling values   电流调节寄存器
	TMC_CHECK_WRITE(0X18,1000);     // cl_upscale=    电流延迟周期寄存器
	TMC_CHECK_WRITE(0X19,100000);       // cl_dnscale=  电流递减延迟周期寄存器
	TMC_CHECK_WRITE(0X07,0X0A400000);        // cl encoder signal configuration with gamma correction and vlimit (catch up velocity)     编码器信号配置寄存器

	TMC_CHECK_WRITE(0X05,0X81);      // cl scaling on  电流调节配置寄存器
	TMC_CHECK_WRITE(0X62,50000);      // 设置编码器速度更新时钟周期数
	TMC_CHECK_WRITE(0X53,500);      // ENC_POS_DEV_TOL 设置编码器ENC_POS_DEV 的最大容差值
	TMC_CHECK_WRITE(0X52,256);				//这里不写的话 TARGET_REACHED不会触发，垂直夹爪由于重力此值需要改大
#endif
	Delayms(10);
	
//	if(TMC4361_MRegStruct.PROTO_STATUS & M_STATUS_SPI_ERR)
//	{
//		while(1)
//		{
//		}
//	}
}
void M_Init()
{
	bsp_2160_cfg();
	bsp_4361_cfg();
}